#ifndef PROCESSOR_H
#define PROCESSOR_H

#include "hnurm_armor/DataType.hpp"
#include "hnurm_armor/TSolver.hpp"
#include "hnurm_armor/armor.hpp"
#include "hnurm_armor/tracker.hpp"
#include <hnurm_interfaces/msg/vision_recv_data.hpp>

#include <chrono>
#include <memory>
#include <vector>

#include <tf2_ros/transform_listener.h>

namespace hnurm
{
class Processor
{
    using Us = std::chrono::microseconds;

public:
    Processor() = default;

    /**
     * @brief Processor中维护了一个Tracker和一个TSolver，参数均在此初始化
     *
     * @param cfg_node 包含tracker_config_node与tracker_config_node
     */
    // explicit Processor(const cv::FileNode &cfg_node);
    explicit Processor(rclcpp::Node::SharedPtr node);

    /* 传入当前云台的pitch和yaw */
    /**
     *
     * @param armors_msg [in]  armors
     * @param pitch pitch
     * @param yaw  yaw
     * @param target_msg [out] target
     */
    // void ProcessArmor(std::vector<Armor> &armors_msg, VisionRecvData &recv_data, TargetInfo &target_msg);
    void
    ProcessArmor(std::vector<Armor> &armors_msg, hnurm_interfaces::msg::VisionRecvData &recv_data, TargetInfo &target_msg);
    std::unique_ptr<Tracker> tracker;
    std::unique_ptr<TSolver> tsolver;

private:
    double s2qxyz_ {}, s2qyaw_ {}, s2qr_ {};

    double r_xyz_factor {}, r_yaw {};

    // for ekf update
    std::chrono::steady_clock::time_point time_elapsed_;
    // time interval between two frames
    double dt_ {};

    // tf
    std::shared_ptr<tf2_ros::Buffer>            tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

    rclcpp::Node::SharedPtr node_;
};

}
#endif  // PROCESSOR_H